/*==============================================================================
Description:
==============================================================================*/

#ifndef __TRACTION_C__
#define __TRACTION_C__

#include "../control_traction/traction.h"

//#include <stdio.h>
//#include <stdarg.h>
//#include <string.h>

#include "../mlib/mlib.h"
#include "../term/term.h"
#include "../supervisor/supervisor.h"
#include "../pal/pal_mem.h"
#include "../input/input.h"
#include "../pal/pal_mem.h"
#include "../app/app.h"


#define  DEBUG_TEST_TRACTION  1
/**********************************************************************

**********************************************************************/
INTERPOLATE_PIONT6 SteerAngleLimitSpeed_Curve;
RMPCNTL			Brake_Ramp;
//INTERPOLATE_PIONT6 MaxTorqeLimit_Curve;

static void BatteryVoltage2Torque(void);


void Traction_Init(void)
{
    pal_memzero( &Traction,         sizeof(Traction)         );
    pal_memzero( &Accelerator_Curve,sizeof(Accelerator_Curve));
    pal_memzero( &Speed_Ramp,       sizeof(Speed_Ramp)       );
    pal_memzero( &pi_Speed2Torque,  sizeof(pi_Speed2Torque)  );
	pal_memzero( &SteerAngleLimitSpeed_Curve,  sizeof(SteerAngleLimitSpeed_Curve)  );
	pal_memzero( &Brake_Ramp,       sizeof(Brake_Ramp)       );	
//    pal_memzero( &MaxTorqeLimit_Curve,sizeof(MaxTorqeLimit_Curve));
	Traction_Parameter_Update();
}
//--------------------------------------------------------------------

/**********************************************************************

**********************************************************************/
void Traction_Control_10ms(void)
{
    
    Traction_Input_Update();
	
#if DEBUG_TEST_TRACTION 	
	PC_Update_Parameter(input_data.v_debug_rx_0);
	PC_get_ParameterSts(output_data.v_debug_4);
#endif

    /*global monitoring function generated from description file supervisor.dsc     */
   // supervisor();
	

    Accelerator2Speed();
    SteerAngle2Speed();
    Speed_Management();

    AccelerationRamp2Speed();

    Speed_Truck2Motor();
    MotorSpeed2Torque();
    BatteryVoltage2Torque();
    ChipTemperture2Torque();
    MotorTemperture2Torque();
	BatterySOC2Torque();
	
    BrakePedal2Torque();
    Torque_Management();
	Traction_Output_10ms();
  //  te_Conv2Diagno();
}

/**********************************************************************

**********************************************************************/
void Traction_Output_10ms(void)
{
    Speed_Motor2Truck();
}

/**********************************************************************

**********************************************************************/
void Traction_Parameter_Update(void)
{
	Traction.Parameter.Wheel_Radius = F32_CONST(0.2);
	Traction.Parameter.Gear_Ratio	 = F32_CONST(16.0);
	
	Traction.Parameter.Speed_Forward_Max = F32_CONST(16500.0);//F32_CONST(18000.0);	 
	// 18km/h--18000mph--->5000mm/s   <===========Tmp limit max speed======sven===========>
	Traction.Parameter.Speed_Backward_Max = F32_CONST(16500.0);//F32_CONST(18000.0);  
	// 18km/h--18000mph--->5000mm/s      <===========Tmp limit max speed======sven===========>
	
	Traction.Parameter.Acceleration_Max = F32_CONST(1.0);/* m/s2*/
	Traction.Parameter.Deceleration_Max = F32_CONST(3.0);/* m/s2*/

	Traction.Parameter.Chip_Temperture_Start = 75;
	Traction.Parameter.Chip_Temperture_End =   90;

	Traction.Parameter.Motor_Temperture_Start = 120;
	Traction.Parameter.Motor_Temperture_End =	140;

	Traction.Parameter.Battery_Voltage_Start =	30;//10000;
	Traction.Parameter.Battery_Voltage_End =   20;//2000;
	Traction.Parameter.Battery_Soc_Start = 30;
	Traction.Parameter.Battery_Soc_End   = 10;

	Traction.Parameter.Torque_max = 350; /* unit is Nm */

	Accelerator_Curve.X[0] = 0;
	Accelerator_Curve.X[1] = 10;
	Accelerator_Curve.X[2] = 50;
	Accelerator_Curve.X[3] = 100;
	Accelerator_Curve.Y[0] = 0;
	Accelerator_Curve.Y[1] = 1800;
	Accelerator_Curve.Y[2] = 9000;
	Accelerator_Curve.Y[3] = 16500;  //18000;   <===========Tmp limit max speed======sven===========>


	SteerAngleLimitSpeed_Curve.X[0]=0;
	SteerAngleLimitSpeed_Curve.X[1]=20;
	SteerAngleLimitSpeed_Curve.X[2]=40;
	SteerAngleLimitSpeed_Curve.X[3]=60;
	SteerAngleLimitSpeed_Curve.X[4]=80;
	SteerAngleLimitSpeed_Curve.X[5]=90; 
	SteerAngleLimitSpeed_Curve.Y[0]=16500 ;/* unit is mph <===========Tmp limit max speed======sven===========>  18000*/  
	SteerAngleLimitSpeed_Curve.Y[1]=8000;
	SteerAngleLimitSpeed_Curve.Y[2]=6000;
	SteerAngleLimitSpeed_Curve.Y[3]=4000;
	SteerAngleLimitSpeed_Curve.Y[4]=3000;
	SteerAngleLimitSpeed_Curve.Y[5]=2000;




	Speed_Ramp.LowerLimit = -(F32)Traction.Parameter.Speed_Backward_Max;
	Speed_Ramp.UpperLimit = (F32)Traction.Parameter.Speed_Forward_Max;

	pi_Speed2Torque.Kp = 0.1f;  //5.0f;
	pi_Speed2Torque.Ki = 0.001f;    //0.005f;
	pi_Speed2Torque.OutMax = (F32)Traction.Parameter.Torque_max;
	pi_Speed2Torque.OutMin =  -(F32) Traction.Parameter.Torque_max;

	Traction.Inner.Enter_Brake_Steps_Num = 5;/* Maximun time to Enter the bake is 5*10ms = 50ms*/
	Traction.Inner.Exit_Brake_Torque_Step = 10;
	Traction.Inner.Brake_Torque_Step = 10;
	Traction.Inner.Brake_Max_Torque = 320;

	
	Traction.Inner.Brake_Ramp_UpperLimit = 5000; /* unit is rpm */
	Traction.Inner.Brake_Ramp_LowerLimit = Get_Truck2Motor(200);/* unit is rpm */
	Traction.Inner.Brake_Ramp_Step = 30;/* unit is rpm */
	Traction.Inner.Brake_Ramp_Half_Step = 15;/* unit is rpm */

	Traction.Parameter.Discharge_MaxTime = 10;/* 10 * 10ms = 100ms */
	Traction.Parameter.Discharge_Torque = 5;/* unit is Nm */
	
    Traction.Inner.Max_Output_Torque_Step = 10;/* unit is Nm */
}


/**********************************************************************

**********************************************************************/
void Traction_Input_Update(void)
{
	
	Traction.Input.Chip_Temperture			= input_data.temperature_motor_1;
	Traction.Input.Motor_Temperture 		= input_data.temperature_motor_2;	
	Traction.Input.Battery_Voltage			= input_data.adc_voltage[mcu_adc_KeySwitch];   //S16<---F32
	Traction.Input.Accelerator_Pedal_Ratio	= input_data.accelerator;	 //U16<---S16
	Traction.Input.Accelerator_Pedal_Ratio_Input= input_data.accelerator;
	Traction.Input.Brake_Pedal_Ratio		= 0;
	Traction.Input.Speed_Direction			= input_data.direction;
	Traction.Input.Motor_Speed_rpm			= input_data.mL_nSen_Can;//input_data.motor_speed;	 //F32<--S16
	Traction.Input.Torque_Enable			= 1;
	if((U8)1 == fStop)
	{
	   Traction.Input.Speed_State  = STOP;
	}
	else if((U8)1 == fCreep)
	{
		Traction.Input.Speed_State	= CREEP;

	}
	else if((U8)1 == fSlow)
	{
		Traction.Input.Speed_State	= SLOW;

	}
	else
	{
		Traction.Input.Speed_State	= NORMAL;

	}

#if DEBUG_TEST_TRACTION  

  // Traction.Input.Speed_State	  = NORMAL; 
   if(((U8)1) == Traction.Inner.Enable_Manul_Acc_Flag)
   {
       if(Traction.Inner.Manul_Acc_Vale > 0)
       {
           Traction.Input.Speed_Direction = direction_switch_state_forward;
		   Traction.Input.Accelerator_Pedal_Ratio = Traction.Inner.Manul_Acc_Vale;
	   }
	   else if(Traction.Inner.Manul_Acc_Vale < 0)
	   {
	       Traction.Input.Speed_Direction = direction_switch_state_reverse;
		   Traction.Input.Accelerator_Pedal_Ratio = Traction.Inner.Manul_Acc_Vale*(-1);
	   }
	   else
	   {
	       Traction.Input.Speed_Direction = direction_switch_state_neutral;
		   Traction.Input.Accelerator_Pedal_Ratio = Traction.Inner.Manul_Acc_Vale;
	   }
       
   }
#endif

	Traction.Input.Steer_Angle = 0;
	
	Traction.Input.Battery_Soc = 100;
	Traction.Input.Disable_Traction = output_data.disable_Trac;
	Traction.Input.Seat_Signal = input_data.seat_switch_1;
//	if((Traction.Input.Seat_Signal < -5)||(0 == input_data.mL_StsMagnetized_Can))
	if(Traction.Input.Seat_Signal < -5)
	{
	    Traction.Input.Accelerator_Pedal_Ratio=0;
	}
}


/**********************************************************************

**********************************************************************/
void Accelerator2Speed(void)
{
    Accelerator_Curve.Input_X = Traction.Input.Accelerator_Pedal_Ratio;
    Interpolate_Calc(&Accelerator_Curve);

    switch(Traction.Input.Speed_Direction)
    {
        case direction_switch_state_forward:
        {
            Traction.Inner.Truck_Speed_mph = (F32)Accelerator_Curve.Output_Y;
            break;
        }
        case direction_switch_state_reverse :
        {
            Traction.Inner.Truck_Speed_mph = -(F32) Accelerator_Curve.Output_Y;
            break;
        }
        default:
        {
            Traction.Inner.Truck_Speed_mph = 0;
        }

    }
}
/**********************************************************************

**********************************************************************/
void SteerAngle2Speed(void)
{
    F32 f32tmp;
    SteerAngleLimitSpeed_Curve.Input_X = Traction.Input.Steer_Angle;//U32<----S16
	Interpolate_Piont6_Calc(&SteerAngleLimitSpeed_Curve);
    f32tmp = (F32)SteerAngleLimitSpeed_Curve.Output_Y;
    switch(Traction.Input.Speed_Direction)
    {
        case direction_switch_state_forward:
        {
			if(Traction.Inner.Truck_Speed_mph > f32tmp)
			{
				Traction.Inner.Truck_Speed_mph = f32tmp;
			}          
            break;
        }
        case direction_switch_state_reverse :
        {
			f32tmp = f32tmp*(-1.0f);
			if(Traction.Inner.Truck_Speed_mph < f32tmp)
			{
			    Traction.Inner.Truck_Speed_mph = f32tmp;
			}
            
            break;
        }
		default:
			break;

    }
}

/**********************************************************************

**********************************************************************/


void Speed_Management(void)
{
    switch(Traction.Input.Speed_State)
    {
 #if DEBUG_TEST_TRACTION !=1       
        case STOP:
        {
            Traction.Inner.Truck_Speed_mph = 0.0f;
        }break;
		
#endif		
        case CREEP:
        {
            Traction.Inner.Truck_Speed_mph = middle_f32 (-2000.0f, Traction.Inner.Truck_Speed_mph, 2000.0f);
        }break;
        case SLOW:
        {
            Traction.Inner.Truck_Speed_mph = middle_f32 (-10000.0f, Traction.Inner.Truck_Speed_mph, 10000.0f);
        }break;
        case NORMAL:
        {
            if(Traction.Inner.Truck_Speed_mph >= 0.0f)
            {
                Traction.Inner.Truck_Speed_mph = middle_f32 (-Traction.Parameter.Speed_Forward_Max, Traction.Inner.Truck_Speed_mph, Traction.Parameter.Speed_Forward_Max);
            }
            else
            {
                Traction.Inner.Truck_Speed_mph = middle_f32 (-Traction.Parameter.Speed_Backward_Max, Traction.Inner.Truck_Speed_mph, Traction.Parameter.Speed_Backward_Max);
            }

        }break;
        default:
        {
            if(Traction.Inner.Truck_Speed_mph >= 0.0f)
            {
                Traction.Inner.Truck_Speed_mph = middle_f32 (-Traction.Parameter.Speed_Forward_Max, Traction.Inner.Truck_Speed_mph, Traction.Parameter.Speed_Forward_Max);
            }
            else
            {
                Traction.Inner.Truck_Speed_mph = middle_f32 (-Traction.Parameter.Speed_Backward_Max, Traction.Inner.Truck_Speed_mph, Traction.Parameter.Speed_Backward_Max);
            }

        }break;
    }
}

/**********************************************************************

**********************************************************************/
void AccelerationRamp2Speed(void)
{
    Traction.Inner.Truck_Speed_mps = Traction.Inner.Truck_Speed_mph/F32_CONST(3600.0);
    Speed_Ramp.TargetValue = Traction.Inner.Truck_Speed_mps;
    Speed_Ramp.Step = Traction.Parameter.Acceleration_Max * F32_CONST(0.01);
    Speed_Ramp.Half_Step = Speed_Ramp.Step * F32_CONST(0.5);

    Speed_Ramp.Step_Deceleration = Traction.Parameter.Deceleration_Max * F32_CONST(0.01);
    Speed_Ramp.Half_Step_Deceleration = Speed_Ramp.Step * F32_CONST(0.5);

	
    Ramp_Calc(&Speed_Ramp);
    Traction.Inner.Truck_Speed_mps = Speed_Ramp.CurrentValue;
}

/**********************************************************************

**********************************************************************/
void Speed_Truck2Motor(void)
{
    F32 Speed_TransmisRatio = 0;
	S16 s16Tmp;

    Speed_TransmisRatio = Traction.Parameter.Gear_Ratio/(ZPI * Traction.Parameter.Wheel_Radius);

    Traction.Inner.Motor_Speed_rpm = F32_CONST(60.0) * Traction.Inner.Truck_Speed_mps * Speed_TransmisRatio;

#if DEBUG_TEST_TRACTION
	s16Tmp = Traction.Inner.Motor_Speed_rpm;
    output_data.v_debug_0[1] = s16Tmp;
	output_data.v_debug_0[2] = Traction.Inner.Truck_Speed_mps;
#endif	

}

/**********************************************************************

**********************************************************************/
void MotorSpeed2Torque(void)
{
    pi_Speed2Torque.Ref = Traction.Inner.Motor_Speed_rpm;
    pi_Speed2Torque.Fbk = Traction.Input.Motor_Speed_rpm;
    if(((U16)1) == Traction.Input.Torque_Enable)
    {
        PI_Calc(&pi_Speed2Torque);
    }
    else
    {
        pi_Speed2Torque.Ui = 0.0f;
        pi_Speed2Torque.Out = 0.0f;
    }
    Traction.Inner.Torque_Out = pi_Speed2Torque.Out;
}

/**********************************************************************

**********************************************************************/
void ChipTemperture2Torque(void)
{
    if(Traction.Input.Chip_Temperture <= Traction.Parameter.Chip_Temperture_Start)
    {
        Traction.Inner.Torque_Chip_Temperture_Limit = Traction.Parameter.Torque_max;
    }
    else if(Traction.Input.Chip_Temperture <= Traction.Parameter.Chip_Temperture_End)
    {
        Traction.Inner.Torque_Chip_Temperture_Limit = muldiv_s16((Traction.Parameter.Chip_Temperture_End - Traction.Input.Chip_Temperture),
                (Traction.Parameter.Torque_max),(Traction.Parameter.Chip_Temperture_End - Traction.Parameter.Chip_Temperture_Start));
    }
    else
    {
        Traction.Inner.Torque_Chip_Temperture_Limit = 0.0f;
    }
}

/**********************************************************************

**********************************************************************/
void MotorTemperture2Torque(void)
{
    if(Traction.Input.Motor_Temperture <= Traction.Parameter.Motor_Temperture_Start)
    {
        Traction.Inner.Torque_Motor_Temperture_Limit = Traction.Parameter.Torque_max;
    }
    else if(Traction.Input.Motor_Temperture <= Traction.Parameter.Motor_Temperture_End)
    {
        Traction.Inner.Torque_Motor_Temperture_Limit = muldiv_s16((Traction.Parameter.Motor_Temperture_End - Traction.Input.Motor_Temperture),
                (Traction.Parameter.Torque_max),(Traction.Parameter.Motor_Temperture_End - Traction.Parameter.Motor_Temperture_Start));
    }
    else
    {
        Traction.Inner.Torque_Motor_Temperture_Limit = 0.0f;
    }
}

/**********************************************************************

**********************************************************************/
static void BatteryVoltage2Torque(void)
{
    if(Traction.Input.Battery_Voltage >= Traction.Parameter.Battery_Voltage_Start)
    {
        Traction.Inner.Torque_Battery_Voltage_Limit = Traction.Parameter.Torque_max;
    }
    else if(Traction.Input.Battery_Voltage >= Traction.Parameter.Battery_Voltage_End)
    {
        Traction.Inner.Torque_Battery_Voltage_Limit = muldiv_s16((Traction.Parameter.Battery_Voltage_End - Traction.Input.Battery_Voltage),
                (Traction.Parameter.Torque_max),(Traction.Parameter.Battery_Voltage_End - Traction.Parameter.Battery_Voltage_Start));
    }
    else
    {
        Traction.Inner.Torque_Battery_Voltage_Limit = 0.0f;
    }
}


/**********************************************************************

**********************************************************************/
void BatterySOC2Torque(void)
{
    if(Traction.Input.Battery_Soc >= Traction.Parameter.Battery_Soc_Start)
    {
        Traction.Inner.Torque_Battery_Soc_Limit = Traction.Parameter.Torque_max;
    }
    else if(Traction.Input.Battery_Soc >= Traction.Parameter.Battery_Soc_End)
    {
        Traction.Inner.Torque_Battery_Soc_Limit = muldiv_s16((Traction.Parameter.Battery_Soc_End - Traction.Input.Battery_Soc),
                (Traction.Parameter.Torque_max),(Traction.Parameter.Battery_Soc_End - Traction.Parameter.Battery_Soc_Start));
    }
    else
    {
        Traction.Inner.Torque_Battery_Soc_Limit = 0.0f;
    }
}



/**********************************************************************

**********************************************************************/
void Clear_Drive_Paramter(void)
{
    Speed_Ramp.CurrentValue =  0; //Traction.Output.Truck_Speed_mps*18/5;
	pi_Speed2Torque.Ui = 0.0f;
	pi_Speed2Torque.Out = 0.0f;

}

/**********************************************************************

**********************************************************************/

void EnterBrakePreprocessParamter(void)
{
    F32 f32tmp;
	f32tmp = Traction.Inner.Brake_Torque_Step;
	if(Traction.Input.Motor_Speed_rpm >= 0)
	{
	    Traction.Inner.Brake_state_Flag = BRAKE_FORWARK;
		if(Traction.Inner.Torque_Out > 0)
		{
			
			Traction.Inner.Enter_Brake_Torque_Step = Traction.Inner.Torque_Out/Traction.Inner.Enter_Brake_Steps_Num;
			if(Traction.Inner.Enter_Brake_Torque_Step < f32tmp)
			{
				Traction.Inner.Enter_Brake_Torque_Step = f32tmp;
			}
			
			if(Traction.Inner.Torque_Out > Traction.Inner.Enter_Brake_Torque_Step)
			{
				Traction.Inner.Torque_Out = Traction.Inner.Torque_Out-Traction.Inner.Enter_Brake_Torque_Step;
			}
			else
			{
				Traction.Inner.Torque_Out = 0;
			}
			
			if(Traction.Inner.Torque_Out > 0)
			{
				Traction.Inner.Exit_Brake_hysteresis_Flg = BRAKE_PREPROCESS_FORWARK;
			}
			else
			{
				Traction.Inner.Exit_Brake_hysteresis_Flg = BRAKE_PREPROCESS_NOT_OPRATON;
			}
			Traction.Inner.Brake_Current_Tourque = Traction.Inner.Torque_Out;
		}
		else
		{
			Traction.Inner.Exit_Brake_hysteresis_Flg = BRAKE_PREPROCESS_NOT_OPRATON;
			Traction.Inner.Brake_Current_Tourque = Traction.Inner.Torque_Out;
		}			   
	}
	else
	{
	    Traction.Inner.Brake_state_Flag = BRAKE_REVERSE;
		f32tmp = f32tmp*(-1.0f);
		if(Traction.Inner.Torque_Out > 0)
		{
			Traction.Inner.Exit_Brake_hysteresis_Flg = BRAKE_PREPROCESS_NOT_OPRATON;
			Traction.Inner.Brake_Current_Tourque = Traction.Inner.Torque_Out;
		}
		else
		{
			Traction.Inner.Exit_Brake_hysteresis_Flg = 1;
			Traction.Inner.Enter_Brake_Torque_Step = Traction.Inner.Torque_Out/Traction.Inner.Enter_Brake_Steps_Num;
			if(Traction.Inner.Enter_Brake_Torque_Step > f32tmp)
			{
				Traction.Inner.Enter_Brake_Torque_Step = f32tmp;
			}
			
			if(Traction.Inner.Torque_Out < Traction.Inner.Enter_Brake_Torque_Step)
			{
				Traction.Inner.Torque_Out = Traction.Inner.Torque_Out-Traction.Inner.Enter_Brake_Torque_Step;
			}
			else
			{
				Traction.Inner.Torque_Out = 0;
			}
	
			if(Traction.Inner.Torque_Out < 0)
			{
				Traction.Inner.Exit_Brake_hysteresis_Flg = BRAKE_PREPROCESS_REVERSE;
			}
			else
			{
				Traction.Inner.Exit_Brake_hysteresis_Flg = BRAKE_PREPROCESS_NOT_OPRATON;
			}
			Traction.Inner.Brake_Current_Tourque = Traction.Inner.Torque_Out;
	
		}
	}

}

/**********************************************************************

**********************************************************************/
void EnterBrakePreprocessTorque(void)
{
    if(BRAKE_PREPROCESS_FORWARK == (BRAKE_PREPROCESS_STATE)Traction.Inner.Exit_Brake_hysteresis_Flg)
    {
        if(Traction.Inner.Brake_Current_Tourque > Traction.Inner.Enter_Brake_Torque_Step)
		{
			Traction.Inner.Brake_Current_Tourque = Traction.Inner.Brake_Current_Tourque - Traction.Inner.Enter_Brake_Torque_Step;
		}
		else
		{
			Traction.Inner.Brake_Current_Tourque = 0;
		}
		
		if(Traction.Inner.Brake_Current_Tourque <= 0)
		{
			Traction.Inner.Exit_Brake_hysteresis_Flg = BRAKE_PREPROCESS_NOT_OPRATON;
			Traction.Inner.Brake_Current_Tourque = 0;
		}
	}
	else
	{
		if(Traction.Inner.Brake_Current_Tourque < Traction.Inner.Enter_Brake_Torque_Step)
		{
			Traction.Inner.Brake_Current_Tourque = Traction.Inner.Brake_Current_Tourque -Traction.Inner.Enter_Brake_Torque_Step;
		}
		else
		{
			Traction.Inner.Brake_Current_Tourque = 0;
		}
		
		if(Traction.Inner.Brake_Current_Tourque >= 0)
		{
			Traction.Inner.Exit_Brake_hysteresis_Flg = BRAKE_PREPROCESS_NOT_OPRATON;
			Traction.Inner.Brake_Current_Tourque = 0;
		}

	}
}


/**********************************************************************
	
**********************************************************************/

void BrakePedal2Torque(void)
{
   F32 f32tmp,f32tmp1;
   f32tmp = Traction.Inner.Brake_Max_Torque;
   if(Traction.Input.Brake_Pedal_Ratio > (U16)(0))
   {
       Traction.Inner.Brake_Rember_Torque_Flag =1;
	   Clear_Drive_Paramter();
	   if((U8)(0) == Traction.Inner.Enter_Brake_First_Flag)
	   {
		   Traction.Inner.Enter_Brake_First_Flag = 1;		   
		   Traction.Inner.Brake_Reverse_Ramp_Flag =0;
		   Traction.Inner.Brake_Forwark_Ramp_Flag =0;
		   EnterBrakePreprocessParamter();
	   }
	   else
	   {
	       if(BRAKE_PREPROCESS_NOT_OPRATON != (BRAKE_PREPROCESS_STATE)Traction.Inner.Exit_Brake_hysteresis_Flg)
	       {
			   Traction.Inner.Brake_Reverse_Ramp_Flag =0;
		       Traction.Inner.Brake_Forwark_Ramp_Flag =0;
	           EnterBrakePreprocessTorque();			   
		   }
		   else
		   {
		        if(BRAKE_FORWARK == (BRAKE_STATE)Traction.Inner.Brake_state_Flag)
		        {
		            Traction.Inner.Brake_Reverse_Ramp_Flag =0;
					//for misra
		           // f32tmp = f32tmp*(-1); 
					
					if(Traction.Input.Motor_Speed_rpm< Traction.Inner.Brake_Ramp_LowerLimit)
					{
					    Traction.Inner.Brake_Forwark_Ramp_Flag =0;
					    Traction.Inner.Brake_Current_Tourque = 0;
					}
					else
					{
						if((U8)(0) == Traction.Inner.Brake_Forwark_Ramp_Flag)
						{
							Traction.Inner.Brake_Forwark_Ramp_Flag = 1;
							Brake_Ramp.CurrentValue = Traction.Input.Motor_Speed_rpm;
							Brake_Ramp.TargetValue = Traction.Input.Motor_Speed_rpm;
							Brake_Ramp.UpperLimit = Traction.Inner.Brake_Ramp_UpperLimit;
							Brake_Ramp.LowerLimit = Traction.Inner.Brake_Ramp_LowerLimit;
							Brake_Ramp.Step = Traction.Inner.Brake_Ramp_Step;
							Brake_Ramp.Half_Step = Traction.Inner.Brake_Ramp_Half_Step;
							Brake_Ramp.Step_Deceleration = Brake_Ramp.Step;
							Brake_Ramp.Half_Step_Deceleration = Brake_Ramp.Half_Step;
						}
						else
						{
							Brake_Ramp.TargetValue = Traction.Input.Motor_Speed_rpm;
						}
						Ramp_Calc(&Brake_Ramp);
						Brake_Ramp.CurrentValue = middle_f32(Brake_Ramp.LowerLimit,Brake_Ramp.CurrentValue,Brake_Ramp.UpperLimit);
						Traction.Inner.Brake_Current_Tourque = Traction.Inner.Brake_Current_Tourque * (Brake_Ramp.CurrentValue - Brake_Ramp.LowerLimit)/(Brake_Ramp.UpperLimit-Brake_Ramp.LowerLimit);
					}

				}
				else
				{
				    Traction.Inner.Brake_Forwark_Ramp_Flag =0;
					if(Traction.Input.Motor_Speed_rpm >(Traction.Inner.Brake_Ramp_LowerLimit*(-1)))
					{
					    Traction.Inner.Brake_Current_Tourque = 0;
						Traction.Inner.Brake_Reverse_Ramp_Flag =0;
					}
					else
					{
					    if((U8)(0) == Traction.Inner.Brake_Reverse_Ramp_Flag)
					    {
					        Traction.Inner.Brake_Reverse_Ramp_Flag = 1;
					        Brake_Ramp.CurrentValue = Traction.Input.Motor_Speed_rpm;
							Brake_Ramp.UpperLimit = Traction.Inner.Brake_Ramp_UpperLimit*(-1);
							Brake_Ramp.LowerLimit = Traction.Inner.Brake_Ramp_LowerLimit*(-1);
							Brake_Ramp.Step = Traction.Inner.Brake_Ramp_Step;
							Brake_Ramp.Half_Step = Traction.Inner.Brake_Ramp_Half_Step;							
							Brake_Ramp.TargetValue = Traction.Input.Motor_Speed_rpm;
							Brake_Ramp.Step_Deceleration =Brake_Ramp.Step;
							Brake_Ramp.Half_Step_Deceleration = Brake_Ramp.Half_Step;
						}
						else
						{
							Brake_Ramp.TargetValue = Traction.Input.Motor_Speed_rpm;
						    
						}
						Ramp_Calc(&Brake_Ramp);
	                    Brake_Ramp.CurrentValue = middle_f32(Brake_Ramp.LowerLimit,Brake_Ramp.CurrentValue,Brake_Ramp.UpperLimit);
					    Traction.Inner.Brake_Current_Tourque = Traction.Inner.Brake_Current_Tourque * (Brake_Ramp.CurrentValue - Brake_Ramp.LowerLimit)/(Brake_Ramp.UpperLimit-Brake_Ramp.LowerLimit);
					}

				}
		   }
	   }	   
	   Traction.Inner.Exit_Brake_Update_Param_Flag =1;
	   Traction.Inner.Torque_Out = Traction.Inner.Brake_Current_Tourque;   
   }
   else
   {
       Traction.Inner.Brake_Forwark_Ramp_Flag =0;
       Traction.Inner.Brake_Reverse_Ramp_Flag =0;
	   Traction.Inner.Enter_Brake_First_Flag = 0;
	   if(((U8)(1) == Traction.Inner.Exit_Brake_Update_Param_Flag)&&((F32)(0) != Traction.Inner.Brake_Current_Tourque))
	   {
	       Traction.Inner.Exit_Brake_Update_Param_Flag =0;		   
		   Traction.Inner.Exit_Brake_hysteresis_Flg = 1;
	   }
	   if((U8)(1) == Traction.Inner.Exit_Brake_hysteresis_Flg)
	   {
	       f32tmp  = Traction.Inner.Brake_Current_Tourque  + Traction.Inner.Exit_Brake_Torque_Step;
		   f32tmp1 = Traction.Inner.Brake_Current_Tourque  - Traction.Inner.Exit_Brake_Torque_Step; 
		   
	       if(Traction.Inner.Torque_Out > f32tmp)
	       {
			   Traction.Inner.Brake_Current_Tourque = f32tmp;
		   }
		   else if(Traction.Inner.Torque_Out < f32tmp1)
		   {
			   Traction.Inner.Brake_Current_Tourque = f32tmp1;
		   }
		   else
		   {
		      Traction.Inner.Exit_Brake_hysteresis_Flg=0;
			  Traction.Inner.Brake_Current_Tourque = 0;
		   }
		   Traction.Inner.Torque_Out = Traction.Inner.Brake_Current_Tourque; 
		   Traction.Inner.Brake_Rember_Torque_Flag =1;
	   }
	   else
	   {
	       Traction.Inner.Brake_Rember_Torque_Flag =0;
	   }
   }

  
}

/**********************************************************************

**********************************************************************/
void Torque_Management(void)
{
   
    Traction.Inner.Torque_Out   = middle_f32 (-Traction.Inner.Torque_Chip_Temperture_Limit, Traction.Inner.Torque_Out, Traction.Inner.Torque_Chip_Temperture_Limit);
    Traction.Inner.Torque_Out   = middle_f32 (-Traction.Inner.Torque_Motor_Temperture_Limit, Traction.Inner.Torque_Out, Traction.Inner.Torque_Motor_Temperture_Limit);
    Traction.Inner.Torque_Out   = middle_f32 (-Traction.Inner.Torque_Battery_Voltage_Limit, Traction.Inner.Torque_Out , Traction.Inner.Torque_Battery_Voltage_Limit);
    Traction.Output.Torque_Out  = middle_f32 (-Traction.Inner.Torque_Battery_Soc_Limit, Traction.Inner.Torque_Out , Traction.Inner.Torque_Battery_Soc_Limit);
	
   //Traction.Output.Torque_Out  =  Traction.Inner.Torque_Out; 

    if(((U16)(0) == Traction.Input.Accelerator_Pedal_Ratio)&&((Traction.Input.Motor_Speed_rpm <10.0f )&&(Traction.Input.Motor_Speed_rpm > -10.0f)))
    {
       Traction.Output.Torque_Out = 0;
	}
	
#if DEBUG_TEST_TRACTION
    if(1 == Traction.Inner.Enable_Manul_Ctrl_Torque_Flag)
    {
        Traction.Output.Torque_Out = Traction.Inner.Manul_Ctrl_Torque;
	}
#endif
	
#if 1
    if((S16)1 == Traction.Input.Disable_Traction) //disall
    {
	    Traction.Output.Torque_Out =0;		
		Clear_Drive_Paramter();
		Traction.Output.Enable_Left_Motor =1;
		Traction.Output.Enable_Left_Motor_Pwm =0;

		Traction.Output.Enable_Right_Motor=0;
		Traction.Output.Enable_Right_Motor_Pwm=0;
		Traction.Inner.Rember_StsMagnetized_Flag =0;
		
	}
	else
	{
	    if((0.0f !=Traction.Output.Torque_Out)||((1 == Traction.Inner.Rember_StsMagnetized_Flag)&&((Traction.Input.Accelerator_Pedal_Ratio_Input >0)||(0 !=Traction.Input.Motor_Speed_rpm))))
	    {
			Traction.Output.Enable_Left_Motor =2;
			Traction.Output.Enable_Left_Motor_Pwm =1;
			
			Traction.Output.Enable_Right_Motor=0;
			Traction.Output.Enable_Right_Motor_Pwm=0;
			Traction.Inner.Rember_StsMagnetized_Flag =1;
			
			if(Traction.Output.Torque_Out > (Traction.Inner.Max_Output_Torque_Step + Traction.Inner.Last_Output_Torque))
			{
			    Traction.Output.Torque_Out = Traction.Inner.Max_Output_Torque_Step + Traction.Inner.Last_Output_Torque;
			}
			else if((Traction.Output.Torque_Out+Traction.Inner.Max_Output_Torque_Step) < Traction.Inner.Last_Output_Torque)
			{
			    Traction.Output.Torque_Out = Traction.Inner.Last_Output_Torque -Traction.Inner.Max_Output_Torque_Step;

			}
			else
			{
				
			}
			
			
			

		}
		else
		{
			Traction.Output.Enable_Left_Motor =1;
			Traction.Output.Enable_Left_Motor_Pwm =0;
			
			Traction.Output.Enable_Right_Motor=0;
			Traction.Output.Enable_Right_Motor_Pwm=0;
			Traction.Inner.Rember_StsMagnetized_Flag =0;

		}


	}

   
#else

    if(0 == Traction.Input.Disable_Traction)
    {
		if(0!=Traction.Output.Torque_Out)
		{
		   output_data.mL_CmdEnable =2;    
		   output_data.mL_CmdPwmEna =1;
		   
		   output_data.mR_CmdEnable =0;
		   output_data.mR_CmdPwmEna =0;    
		}
		else
		{
			output_data.mL_CmdEnable =1;		
			output_data.mL_CmdPwmEna =0;
			
			output_data.mR_CmdEnable =0;
			output_data.mR_CmdPwmEna =0;
		}

	}
	
	if(1 == Traction.Input.Disable_Traction)
	{
	    Traction.Output.Torque_Out =0;
		Clear_Drive_Paramter();
	}

#endif

	if(0 == input_data.mL_StsMagnetized_Can)
	{
	    Traction.Output.Torque_Out =0;
		Clear_Drive_Paramter();
	}





	
	if(1 == Traction.Inner.Brake_Rember_Torque_Flag)
	{
	    Traction.Inner.Brake_Current_Tourque = Traction.Output.Torque_Out;
	}
	else
	{
	   Traction.Inner.Brake_Current_Tourque =0;
	}

	 Traction.Inner.Last_Output_Torque = Traction.Output.Torque_Out;

}


/**********************************************************************

**********************************************************************/
void Speed_Motor2Truck(void)
{
    F32 Speed_TransmisRatio = 0;

    Speed_TransmisRatio = Traction.Parameter.Gear_Ratio/(ZPI * Traction.Parameter.Wheel_Radius);

    Traction.Output.Truck_Speed_mps = Traction.Input.Motor_Speed_rpm/(F32_CONST(60.0) * Speed_TransmisRatio);
	Traction.Output.Truck_Speed_mph = Traction.Output.Truck_Speed_mps * F32_CONST(3600.0);
}
/**********************************************************************

**********************************************************************/
void Interpolate_Piont6_Calc(INTERPOLATE_PIONT6 *p)
{
	  if (p->Input_X < p->X[0])
	  {
		  p->Output_Y = 0;
	  }
	  else if (p->Input_X < p->X[1])
	  {
		  p->Output_Y = ((p->Input_X-p->X[0])*(p->Y[1]-p->Y[0]))/(p->X[1]-p->X[0]) + p->Y[0];
	  }
	  else if (p->Input_X < p->X[2])
	  {
		  p->Output_Y = ((p->Input_X-p->X[1])*(p->Y[2]-p->Y[1]))/(p->X[2]-p->X[1]) + p->Y[1];
	  }
	  else if (p->Input_X < p->X[3])
	  {
		  p->Output_Y = ((p->Input_X-p->X[2])*(p->Y[3]-p->Y[2]))/(p->X[3]-p->X[2]) + p->Y[2];
	  }
	  else if (p->Input_X < p->X[4])
	  {
		  p->Output_Y = ((p->Input_X-p->X[3])*(p->Y[4]-p->Y[3]))/(p->X[4]-p->X[3]) + p->Y[3];
	  }
	  else if (p->Input_X < p->X[5])
	  {
		  p->Output_Y = ((p->Input_X-p->X[4])*(p->Y[5]-p->Y[4]))/(p->X[5]-p->X[4]) + p->Y[4];
	  }	  
	  else
	  {
		  p->Output_Y = p->Y[5];
	  }
}


/**********************************************************************

**********************************************************************/
F32  Get_Truck2Motor(F32 Speed_mph)
{
    F32 Speed_TransmisRatio = 0;
    F32 Motor_Speed_rpm=0;
    Speed_TransmisRatio = Traction.Parameter.Gear_Ratio/(ZPI * Traction.Parameter.Wheel_Radius);

    Motor_Speed_rpm = Speed_mph * Speed_TransmisRatio/ F32_CONST(60.0);
	return Motor_Speed_rpm;
}


/**********************************************************************

**********************************************************************/

void PC_Update_Parameter(U16 *MsgData)
{
   U16 ParamValue;
   U8  ParamID;
   U8  ModuleID;
   F32 F32tmp;
 
   if(NULL == MsgData)
   {
      return;
   }
   else
   {
      ModuleID = (*MsgData) & 0xff;
	  ParamID = ((*MsgData)>>8) & 0xff; 
      if(0x01 != ModuleID)
      {
          return;
	  }
	  else
	  { 
		 //ParamValue =*(MsgData+1);
		 ParamValue =MsgData[1];
		 Traction.Inner.PC_Read_ID = PARMID_NOT_OPRATON;
		 switch(ParamID)
		 {
			 case PARMID_PIONT1_X:
			 {
				 Accelerator_Curve.X[1] = (U32)ParamValue;
			 }
			 break;
			 case PARMID_PIONT2_X:
			 {
				 Accelerator_Curve.X[2] = (U32)ParamValue;
			 }
			 break;			 
			 case PARMID_PIONT3_X:
			 {
				 Accelerator_Curve.X[3] = (U32)ParamValue;
			 }
			 break;
			 case PARMID_PIONT1_Y:
			 {
				 Accelerator_Curve.Y[1] = (U32)ParamValue;
			 }
			 break;
			 case PARMID_PIONT2_Y:
			 {
				 Accelerator_Curve.Y[2] = (U32)ParamValue;
			 }
			 break;			 
			 case PARMID_PIONT3_Y:
			 {
				 Accelerator_Curve.Y[3] = (U32)ParamValue;
			 }
			 break;
			 case PARMID_PI_KP:
			 {
			 	 F32tmp = ParamValue;
				 F32tmp = F32tmp/100.0f;
				 pi_Speed2Torque.Kp = F32tmp;
			 }
			 break;			 
			 case PARMID_PI_KI:
			 {
				 F32tmp = ParamValue;
				 F32tmp = F32tmp/1000.0f;
				 pi_Speed2Torque.Ki = F32tmp;
			 }
			 break;
			 case PARMID_READ_ID:
			 {
			 	Traction.Inner.PC_Read_ID = (ParamID_Type)ParamValue;
			 }
			 break;
			 case PARMID_ENABLE_MANUAL_CTRL_TORQUE:
			 {
			 	Traction.Inner.Enable_Manul_Ctrl_Torque_Flag = (U8)ParamValue;
			 }
			 break;
			 case PARMID_MANUL_TORQUE:
			 {
			 	Traction.Inner.Manul_Ctrl_Torque = (F32)(ParamValue-(U16)1000);
			 }
			 break;
			 case PARMID_ENABLE_MANUL_CTRL_ACC://19
			 {
			 	Traction.Inner.Enable_Manul_Acc_Flag = (U8)ParamValue;
			 }
			 break;
			 case PARMID_MANUL_ACC://20
			 {
			 	Traction.Inner.Manul_Acc_Vale =(S16) (ParamValue-(U16)100);
			 }
			 break;

			 
			 
			 default:
			 	break;
		 }
		 
	  }
   }

}

void PC_get_ParameterSts(U16 *TX_MsgData)
{
	U16 ParamValue;
   // U8 ParamID;	
	if(NULL == TX_MsgData)
	{
		return;
	}
	else
	{
		//*TX_MsgData = (Traction.Inner.PC_Read_ID<<8)|0x01;
		TX_MsgData[0] = Traction.Inner.PC_Read_ID<<8;
		TX_MsgData[0] = TX_MsgData[0] | 0x01;
		switch(Traction.Inner.PC_Read_ID)
		{
			case PARMID_PIONT1_X:
			{
				ParamValue = Accelerator_Curve.X[1];
				//*(TX_MsgData + 1) = ParamValue ;
				TX_MsgData[1] = ParamValue;
			}
			break;
			case PARMID_PIONT2_X:
			{
				ParamValue = Accelerator_Curve.X[2];
				//*(TX_MsgData + 1) = ParamValue ;
				TX_MsgData[1] = ParamValue;
			}break;			 
			case PARMID_PIONT3_X:
			{
				ParamValue = Accelerator_Curve.X[3];
				//*(TX_MsgData + 1) = ParamValue ;
				TX_MsgData[1] = ParamValue;
			}
			break;
			case PARMID_PIONT1_Y:
			{
				ParamValue = Accelerator_Curve.Y[1];
				//*(TX_MsgData + 1) = ParamValue ;
				TX_MsgData[1] = ParamValue;
			}
			break;
			case PARMID_PIONT2_Y:
			{
				ParamValue = Accelerator_Curve.Y[2];
				//*(TX_MsgData + 1) = ParamValue ;
				TX_MsgData[1] = ParamValue;
			}
			break;			 
			case PARMID_PIONT3_Y:
			{
				ParamValue = Accelerator_Curve.Y[3];
				//*(TX_MsgData + 1) = ParamValue ;
				TX_MsgData[1] = ParamValue;
			}
			break;
			case PARMID_PI_KP:
			{
				ParamValue =pi_Speed2Torque.Kp*100;
				//*(TX_MsgData + 1) = ParamValue ;
				TX_MsgData[1] = ParamValue;
			}
			break;			 
			case PARMID_PI_KI:
			{
				ParamValue =pi_Speed2Torque.Ki*1000;
				//*(TX_MsgData + 1) = ParamValue ;
				TX_MsgData[1] = ParamValue;
			}
			break;

			case PARMID_ENABLE_MANUAL_CTRL_TORQUE:
			{
				ParamValue =Traction.Inner.Enable_Manul_Ctrl_Torque_Flag;
				//*(TX_MsgData + 1) = ParamValue ;
				TX_MsgData[1] = ParamValue;
			}
			break;

			case PARMID_MANUL_TORQUE:
			{
				ParamValue = Traction.Inner.Manul_Ctrl_Torque+1000;
				//*(TX_MsgData + 1) = ParamValue ;	
				TX_MsgData[1] = ParamValue;
			}
			break;	


			case PARMID_ENABLE_MANUL_CTRL_ACC://19
			{
				ParamValue = Traction.Inner.Enable_Manul_Acc_Flag;
				//*(TX_MsgData + 1) = ParamValue ;
				TX_MsgData[1] = ParamValue;
			}
			break;

			case PARMID_MANUL_ACC://20
			{
				ParamValue = Traction.Inner.Manul_Acc_Vale+100;
				//*(TX_MsgData + 1) = ParamValue ;
				TX_MsgData[1] = ParamValue;
			}
			break;


			
			default:
			break;
		}
	}

}	


#endif

//--- end of file ----------------------------------------------------
